﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Mahanet.Abyss.PathCalculation.Aimer;
using System.Threading;

namespace Mahanet.Abyss.PathCalculation
{
	public class GunBombardier
	{
		#region Consts

		private const int TIME_TO_WAIT_FOR_GUN_TO_AIM_IN_MS = 1000;
		private const int TIME_TO_WAIT_BEFORE_QUERYING_PATH_RESULT_IN_MS = 2000;

		// This is 17 for absolutely no good reason.
		// Technically, the higher this is the more accurate it should be - as long as there are enough data points.
		// Too few data points in relation to the approximation degree will cause large errors.
		// Notice the functions here are mostly not polynomial:
		//  * DistanceToTravelTime is the root of a second degree polynomial assuming no air friction;
		//  * Angular Skews are probably some form of sinosidal
		// For that reason, we need a reasonably high degree so that we can approximate these functions effectively with a polynomial.
		private const int POLYNOMIAL_APPROXIMATION_DEGREE = 17;

		#endregion

		#region Fields

		private List<DataPoint<double, double>> m_DistanceToTravelTimeDataPoints = new List<DataPoint<double, double>>();
		private List<DataPoint<double, double>> m_DistanceToHorizontalAngularSkewDataPoints = new List<DataPoint<double, double>>();
		private List<DataPoint<double, double>> m_DistanceToVerticalAngularSkewDataPoints = new List<DataPoint<double, double>>();

		private Func<double, double> m_DistanceToTravelTime;
		private Func<double, double> m_DistanceToHorizontalAngularSkew;
		private Func<double, double> m_DistanceToVerticalAngularSkew;

		#endregion

		#region C'tor

		public GunBombardier()
		{
			// Initialization values; these are not exact, and will be perfected during calibration
			m_DistanceToHorizontalAngularSkew = d => 0;
			m_DistanceToVerticalAngularSkew = d => 0;
			m_DistanceToTravelTime = d => d / 200;
		}

		#endregion

		#region Public Properties

		public Coordinate3D GunRotationCenter { get; set; }

		public Plane3D GunHorizontalZeroAngleDirection { get; set; }

		public Plane3D GunVerticalZeroAngleDirection { get; set; }

		#endregion

		#region Public Methods

		public GunFiringParams AimAtPoint(Coordinate3D targetPoint, double interceptionTime)
		{
			double interceptionDistance = targetPoint.DistanceTo(GunRotationCenter);

			double horizontalAngleSkew = m_DistanceToHorizontalAngularSkew(interceptionDistance);
			double verticalAngleSkew = m_DistanceToVerticalAngularSkew(interceptionDistance);

			double fireHorizontalAngle;
			double fireVerticalAngle;
			GetFireDirectionAngles(targetPoint, out fireHorizontalAngle, out fireVerticalAngle);

			double travelTime = m_DistanceToTravelTime(interceptionDistance);
			double fireTime = interceptionTime - travelTime;

			return new GunFiringParams(fireHorizontalAngle + horizontalAngleSkew, fireVerticalAngle + verticalAngleSkew, fireTime);
		}

		// Assumes the gun properties (rotation center, zero angle directions) are already calibrated
		public void Recalibrate(Coordinate3D bullseye, IGunFacade3D gun, IFiringFeedbackTeacher firingTeacher, IClock clock)
		{
			var firingParams = AimAtPoint(bullseye, 0);
			gun.ChangeGunAngle(firingParams.HorizontalAngle, firingParams.VerticalAngle);
			Thread.Sleep(TIME_TO_WAIT_FOR_GUN_TO_AIM_IN_MS);

			double fireTime = clock.GetCurrentTime();
			firingTeacher.PrepareForFire();
			gun.Fire();
			Thread.Sleep(TIME_TO_WAIT_BEFORE_QUERYING_PATH_RESULT_IN_MS);

			var fireResult = firingTeacher.GetFireResults();
			CreateNewDataPointsFromResults(bullseye, firingParams, fireTime, fireResult);

			LearnFromDataPoints();
		}

		#endregion

		#region Private Methods

		private void GetFireDirectionAngles(Coordinate3D targetPoint, out double fireHorizontalAngle, out double fireVerticalAngle)
		{
			Vector3D fireVector = new Vector3D(GunRotationCenter, targetPoint);
			fireHorizontalAngle = GunHorizontalZeroAngleDirection.AngleTo(fireVector);
			fireVerticalAngle = GunVerticalZeroAngleDirection.AngleTo(fireVector);
		}

		private void CreateNewDataPointsFromResults(Coordinate3D bullseye, GunFiringParams firingParams, double fireTime, List<DataPoint<double, Coordinate3D>> fireResult)
		{
			foreach (var dataPoint in fireResult)
			{
				double hitTime = dataPoint.Paramter;
				Coordinate3D hitPoint = dataPoint.Value;
				double distance = bullseye.DistanceTo(hitPoint);

				GetTravelTimeDataPoint(fireTime, hitTime, distance);

				GetAngularSkewDataPoints(firingParams, hitPoint, distance);
			}
		}

		private void GetAngularSkewDataPoints(GunFiringParams firingParams, Coordinate3D hitPoint, double distance)
		{
			double horizontalAngle;
			double verticalAngle;
			GetFireDirectionAngles(hitPoint, out horizontalAngle, out verticalAngle);

			double verticalSkew = firingParams.VerticalAngle - verticalAngle;
			double horizontalSkew = firingParams.HorizontalAngle - horizontalAngle;

			m_DistanceToHorizontalAngularSkewDataPoints.Add(new DataPoint<double, double>(distance, horizontalSkew));
			m_DistanceToVerticalAngularSkewDataPoints.Add(new DataPoint<double, double>(distance, verticalSkew));
		}

		private void GetTravelTimeDataPoint(double fireTime, double hitTime, double distance)
		{
			double travelTime = hitTime - fireTime;
			m_DistanceToTravelTimeDataPoints.Add(new DataPoint<double, double>(distance, travelTime));
		}

		private void LearnFromDataPoints()
		{
			m_DistanceToTravelTime = LearnFromDataPoints(m_DistanceToTravelTimeDataPoints);
			m_DistanceToHorizontalAngularSkew = LearnFromDataPoints(m_DistanceToHorizontalAngularSkewDataPoints);
			m_DistanceToVerticalAngularSkew = LearnFromDataPoints(m_DistanceToVerticalAngularSkewDataPoints);
		}

		private Func<double, double> LearnFromDataPoints(List<DataPoint<double, double>> dataPoints)
		{
			alglib.polynomialfitreport report;
			int info = 0;
			double[] dValues = dataPoints.Select(dataPoint => dataPoint.Paramter).ToArray();
			double[] aValues = dataPoints.Select(dataPoint => dataPoint.Value).ToArray();

			alglib.barycentricinterpolant aApproximation;
			alglib.polynomialfit(dValues, aValues, POLYNOMIAL_APPROXIMATION_DEGREE, out info, out aApproximation, out report);

			return d => alglib.barycentriccalc(aApproximation, d);
		}

		#endregion
	}
}
